﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Drifting.Foundation;
using Drifting.MotionPlanning.Primitives;

namespace Drifting.Graphics.EffectObjects
{
    public class Tetrahedron : EffectObject
    {
        static short[] quad_Indices;
        private VertexPositionColor[] vertices;
        private BasicEffect effects;
        private Matrix configuration;

        // private VertexPositionNormalTexture[] vtexture;
        // private short[] v_indices;

        public double[] Config
        {
            set {
                float x = (float)value[0];
                float y = (float)value[1];
                float z = (float)value[2];
                float rx = (float)value[3];
                float ry = (float)value[4];
                float rz = (float)value[5];

                float angle = (float)Math.Sqrt(rx * rx + ry * ry + rz * rz);
                if (angle != 0)
                {
                    rx /= angle;
                    ry /= angle;
                    rz /= angle;
                    configuration = Matrix.CreateFromAxisAngle(new Vector3(rx, ry, rz), angle) * Matrix.CreateTranslation(x, y, z);
                }
            }
        }

        public Tetrahedron(float scale, Color color, GraphicsDevice device)
            : base(device)
        {
            double sq3 = Math.Sqrt(3);
            double sq6 = Math.Sqrt(6);

            vertices = new VertexPositionColor[4];

            Vector3 v0 = new Vector3(0.5f * scale, (float)(-sq3 / 6) * scale, (float)(-sq6 / 12) * scale);
            Vector3 v1 = new Vector3(-0.5f * scale, (float)(-sq3 / 6) * scale, (float)(-sq6 / 12) * scale);
            Vector3 v2 = new Vector3(0, (float)(sq3 / 3) * scale, (float)(-sq6 / 12) * scale);
            Vector3 v3 = new Vector3(0, 0, (float)(sq6 / 4) * scale);

            vertices[0] = new VertexPositionColor(v0, color);
            vertices[1] = new VertexPositionColor(v1, color);
            vertices[2] = new VertexPositionColor(v2, color);
            vertices[3] = new VertexPositionColor(v3, color);


            /*
            /////////// normal vertex /////////////

            Vector3 v01 = v1 - v0;
            Vector3 v12 = v2 - v1;
            Vector3 w012 = MathAssistant.vector_cross3(v01, v12);

            Vector3 v10 = -v01;
            Vector3 v03 = v3 - v0;
            Vector3 w103 = MathAssistant.vector_cross3(v10, v03);

            Vector3 v21 = -v12;
            Vector3 v13 = v3 - v1;
            Vector3 w213 = MathAssistant.vector_cross3(v21, v13);

            Vector3 v30 = -v03;
            Vector3 v02 = v2 - v0;
            Vector3 w302 = MathAssistant.vector_cross3(v30, v02);
            
            */

            if (quad_Indices == null)
            {
                quad_Indices = new short[] { 0, 1, 2, 
                                             2, 3, 0,
                                             2, 1, 3,
                                             1, 0, 3};
                
            }
            effects = new BasicEffect(device);

            configuration = Matrix.Identity;
        }

        public void SetConfig(Matrix m)
        {
            configuration = m;
        }

        override public void Draw(Matrix view, Matrix projection)
        {
            effects.VertexColorEnabled = true;
            effects.World = configuration;
            effects.View = view;
            effects.Projection = projection;
            effects.CurrentTechnique.Passes[0].Apply();

            device.DrawUserIndexedPrimitives(PrimitiveType.TriangleList, vertices, 0, 4, quad_Indices, 0, 4);
        }

        public void Draw(Matrix view, Matrix projection, Color color)
        {
            effects.VertexColorEnabled = true;
            effects.World = configuration;
            effects.View = view;
            effects.Projection = projection;
            effects.CurrentTechnique.Passes[0].Apply();

            if (color != null)
            {
                vertices[0].Color = color;
                vertices[1].Color = color;
                vertices[2].Color = color;
                vertices[3].Color = color;
            }
            device.DrawUserIndexedPrimitives(PrimitiveType.TriangleList, vertices, 0, 4, quad_Indices, 0, 4);
        }
        
        public void Draw(Matrix view, Matrix projection, Matrix config)
        {
            effects.VertexColorEnabled = true;
            effects.World = config;
            effects.View = view;
            effects.Projection = projection;
            effects.CurrentTechnique.Passes[0].Apply();

            device.DrawUserIndexedPrimitives(PrimitiveType.TriangleList, vertices, 0, 4, quad_Indices, 0, 4);
        }

        public void Draw(Matrix view, Matrix projection, Matrix config, Color color)
        {
            effects.VertexColorEnabled = true;
            effects.World = config;
            effects.View = view;
            effects.Projection = projection;
            effects.CurrentTechnique.Passes[0].Apply();

            if (color != null)
            {
                vertices[0].Color = color;
                vertices[1].Color = color;
                vertices[2].Color = color;
                vertices[3].Color = color;
            }
            device.DrawUserIndexedPrimitives(PrimitiveType.TriangleList, vertices, 0, 4, quad_Indices, 0, 4);
        }

        public void Draw(Matrix view, Matrix projection, float[] q, Color color)
        {
            Matrix config = Matrix.CreateTranslation(q[0], q[1], q[2]);

            effects.VertexColorEnabled = true;
            effects.World = config;
            effects.View = view;
            effects.Projection = projection;
            effects.CurrentTechnique.Passes[0].Apply();

            if (color != null)
            {
                vertices[0].Color = color;
                vertices[1].Color = color;
                vertices[2].Color = color;
                vertices[3].Color = color;
            }
            device.DrawUserIndexedPrimitives(PrimitiveType.TriangleList, vertices, 0, 4, quad_Indices, 0, 4);
        }
    }
}
